package MULE.Lokalisationsmodul;

import MULE.Bewegungsmodul.Engine;
import lejos.geom.Point;

public class Lokalisation {

	// Positionswerte zur Positionsbestimmung,
	// Orientierung 0 in positiver x-Achsenrichtung
	private Engine Getriebe;
	protected Point point;
	protected double orientation;
	// absolute Drehung
	protected double rotationAbs;
	// gibt an, ob die Position aktuell ist
	private boolean positionIsUpdated;
	private double axleLength;

	// zurueckgelegte Entfernung pro Motortick
	private double distancePerDeg;
	
	public Lokalisation(Engine Getriebe){
		this.Getriebe = Getriebe;
	}
	
	//Positionsbestimmung mit Radumsetzung ---------------------------------------
	/**
	 * Setzt die gespeicherte Pose zurueck auf 0,0,0
	 */
	public void resetPos() {
		this.point.x = 0;
		this.point.y = 0;
		this.orientation = 0;
		// Zuruecksetzen der Motordrehzahlen
		this.Getriebe.MotorRight.resetTachoCount();
		this.Getriebe.MotorLeft.resetTachoCount();
		this.setPositionIsUpdated(true);
	}

	/**
	 * setzt die absolute gedrehte Orientierung zurueck
	 */
	public void resetRotationAbs() {
		this.rotationAbs = 0;
	}

	/**
	 * Positionsberechnung
	 */
	public void updatePos() {
		// Strecke rechtes Rad (deltasr)
		double distanceRightWheel = Getriebe.MotorRight.getTachoCount() * distancePerDeg;
		// Strecke linkes Rad (deltasl)
		double distanceLeftWheel = Getriebe.MotorLeft.getTachoCount() * distancePerDeg;

		// Zuruecksetzen der Motordrehzahlen
		Getriebe.MotorRight.resetTachoCount();
		Getriebe.MotorLeft.resetTachoCount();

		// Strecke Robotermittelpunkt (deltasm)
		double distanceMiddle = (distanceRightWheel + distanceLeftWheel) / 2;

		double radius = 0;
		double deltaTheta = 0;
		double deltaX = 0;
		double deltaY = 0;
		double deltaS = 0;

		if (distanceRightWheel != distanceLeftWheel) {
			radius = (distanceMiddle * this.axleLength)
					/ (distanceRightWheel - distanceLeftWheel);
			if (radius != 0) {
				// 1. Fall: Kurvenfahrt
				deltaTheta = (distanceMiddle / radius); // rad
				deltaS = 2 * radius * Math.sin(deltaTheta / 2);
				deltaX = deltaS * Math.cos((deltaTheta / 2) + orientation);
				deltaY = deltaS * Math.sin((deltaTheta / 2) + orientation);

			} else {
				// 2. Fall: Drehen auf der Stelle
				deltaTheta = 2 * distanceRightWheel / this.axleLength;
			}
		} else {
			// 3. Fall: Geradeausfahrt
			deltaX = distanceMiddle * Math.cos(orientation);
			deltaY = distanceMiddle * Math.sin(orientation);
		}
		// Aktualisieren der Pose
		this.point.translate((float)deltaX, (float)deltaY);
		orientation = (orientation + deltaTheta) % (2 * Math.PI);
		rotationAbs += deltaTheta;

		setPositionIsUpdated(true);
	}

	// SETTER UND GETTER
	/**
	 * 
	 * @return
	 */
	public Point getPosition() {
		return this.point;
	}
	
	/**
	 * 
	 * @return
	 */
	public double getOrientation() {
		return Math.toDegrees(orientation);
	}
	
	/**
	 * 
	 * @return
	 */
	public double getRotationAbs() {
		return Math.toDegrees(rotationAbs);
	}

	/**
	 * 
	 * @param positionIsUpdated
	 */
	public void setPositionIsUpdated(boolean positionIsUpdated) {
		this.positionIsUpdated = positionIsUpdated;
	}

	/**
	 * 
	 * @return
	 */
	public boolean isPositionIsUpdated() {
		return positionIsUpdated;
	}
	
}
